
// TurtleSquare.java
import gin.and.tonic.*;


public class Uranium 
{

    private DragonRobot robot = new DragonRobot();
    UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
    TouchSensor ts = new TouchSensor(SensorPort.S2);
    LightSensor rightEye = new LightSensor(SensorPort.S4);
    LightSensor leftEye = new LightSensor(SensorPort.S3);
    Motor arm = new Motor(MotorPort.A);
    int v = SharedConstants.BRIGHTDEGREE;//420;//360:295//400:260//420;//blue river
    double r = 0.05;
    int diff = 20;
    int spotdistance = 7;//22;//25;
    int walldistance = 7;
    int black = 420;
    int backcount = 0;
    int stopcount=0;
    

    Uranium()
    {


        robot.addPart(rightEye);
        robot.addPart(leftEye);
        robot.addPart(us);
        robot.addPart(ts);
        rightEye.activate(true);
        leftEye.activate(true);

        arm.setSpeed(25);
        robot.addPart(arm);

        /*Mission uranium*/
        robot.setTurtleSpeed(60);


        robot.forward(260); //get out of the base
        robot.right(70);
        robot.forward(180);
        robot.left(18);
        //robot.forward(100);

        arm.rotateTo(130);//130
        arm.getMot().lock(30);
        riverToSpot();//go to the spot
        arm.rotateTo(-50);
        robot.left(173);
        robot.forward(190);
        spotToRiver();
        robot.right(25);
        robot.forward(400);
        


        /*
        robot.backward(100);
        robot.left(12);
        spotToWall();//go back to base
        
        robot.forward(100);
        robot.left(-30);
        robot.backward(330);
        robot.right(-30);
        robot.backward(90);
        robot.right(-60);
        robot.backward(400);
        */

        




        robot.exit();

    }

    private void riverToSpot()
    {
        backcount = 0;
        robot.getGear().forward();
        while (true)
        {
            int leftValue = leftEye.getValue();
            int rightValue = rightEye.getValue();
            int distance = us.getDistance();
            distance = us.getDistance();

            if (distance <= spotdistance)
            {
                stopcount++;
                if(stopcount>=5)
                {
                    robot.getGear().stop();
                break;// in front of the wall
                }
                continue;
            }

           
            if (leftValue < v && rightValue < v)//both dark
            {
                robot.getGear().forward();
                continue;
            }
            
            if (leftValue < v && rightValue > v)
            {
                robot.getGear().leftArc(r);
                continue;
            }
            if (leftValue > v && rightValue < v)
            {
                robot.getGear().rightArc(r);
                continue;
            }
             if (leftValue > v&& rightValue > v)//both bright
            {
               // robot.getGear().backward();
                continue;
            }
            
        }
    }

    private void spotToRiver()
    {
        backcount = 0;
        robot.getGear().forward();
        while (true)
        {
           
            int leftValue = leftEye.getValue();
            int rightValue = rightEye.getValue();

             if (leftValue < v && rightValue < v)
            {
                robot.forward(10);
                leftValue = leftEye.getValue();
                rightValue = rightEye.getValue();
                if (leftValue < v && rightValue < v)
                {
                    robot.getGear().stop();
                    return;
                }
                continue;
            }
            if (rightValue > v)  //  Bright
                robot.getGear().rightArc(r);
              else
                robot.getGear().leftArc(r);

        }
        
    }

  

   

    public static void main(String[] args)
    {

        new Uranium();
    }
}
